/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.28
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 (1) 华磊 2019.1.5 把读取的dh参数单位改为角度和mm
 (2) 华磊 2019.1.14 不再使用movingLineNumber和isLineFinished
 (3) 华磊 2019.1.16 添加线程锁mcMutex用于保护类内的变量
 (4) 华磊 2019.2.18 寸动和移动到点只能在T1模式下运行
 (5) 华磊 2019.3.4 把链式模式放在坐标系模块中

 6) 华磊　2019.6.17 重命名　getMotorCommand 为　algorithmFunctionCall.
                                  重命名　getMotorData 为translateJointDataToMotorData
７）华磊，　2019.06.21。　重命名了一些函数，使含义更加清晰。
　　　　　　　　　　　　　因为修改了　MotorInterface\ internalSafe\ JointDecouple\ 接口，导致有相应修改。
                            　　　　　　增加了jointDecouple时的数据记录分析功能。




  兼容版本需要处理，
１）addTrajectoryCommand　改为　addTrajectoryCommand
２）programEngine屏蔽了　//        pointData = programMcController->mcControllerInterpreter->pointData;
          //        programMcController->mcControllerInterpreter->addMcInterpreterRegData(regData);
                PointPro pointPro;
                programMcController->mcControllerInterpreter->getCurrentCoordinatePoint_Feedback(pointPro);
           // programMcController->mcControllerInterpreter->
           // tempPrValue.resize(pointPro.positionValue.size());

           userCoordinateList直接访问，替换为getUserCoorcinateList();
           toolCoordinateList直接访问，替换为toolUserCoorcinateList();

           << programMcController->mcControllerInterpreter->runningLineNumber 替换为getRunningNumber
3) GlobalDefine 结构体　JointAttribute 修改　torque ,包括配置文件mcinterpreter.xml. 添加了新结构体
4) TcrController.pro
      include( ../MotionServer/MotionServer.pri )
 ***************************************************************************
 *  @file mccontrollerinterpreter.h
 *  功能:算法模块总接口
 *
 *                                                                         *
 ***************************************************************************/
#ifndef MCCONTROLLERINTERPRETER_H
#define MCCONTROLLERINTERPRETER_H

#include "pointdata.h"
#include "regdata.h"
#include  "MacroDefine/GeneralDefine.h"
#include  "controllercommondefine.h"
#include "trajectoryudefine.h"
#include "JointDecouple/dataanalysizer.h"
#include "JointDecouple/dataanalysizerdouble.h"
#include "AxisDefines.h"
#include "qstring.h"
#include "qqueue.h"
#include "robotattribute.h"
#include "motionmessage.h"
#include "JointDecouple/jointdecouple.h"
#include "vehicle/vehicledefine.h"

class TrajectoryServer;
class JogControl;
using namespace KDL;
class MotionForm;
class RosApi;
class PositionRegulatorGroup;
class RobotIo;
class MathAuthorityAllowance;
/**
 * @brief The McControllerInterpreter class 算法模块总接口
 */
class McControllerInterpreter: public DomParser
{
public:
    /**
     * @brief McControllerInterpreter构造函数，用于初始化简单的变量
     * @param _robotId 获取机器人的id
     */
    McControllerInterpreter(int robotIdIn,RegData *regDataIn,RobotIo* ioControllerIn);
    /**
     * @brief ~McControllerInterpreter析构函数
     */
    ~McControllerInterpreter();

public:
    int getArmMovingFlag(int &lineIdOut,E_MOVING_FLAG &extFlagOut);
    int addCircle(int idIn, std::vector<double> posIn, std::vector<double> centerIn,
                  std::vector<double> normalIn, double vel, int turnsIn, int velTypeIn);
    int resetAlgroCallTimeMeasure();
    int getAlgroCallTimeMeasureDebug(vector<string> &comment, vector<double> &timeInfo);
    int setTpNetFeedScale(double scaleIn);
    int getRobotCommonPara(RobotCommonParameter &robotParameterOut);
    int getEmcDebugInfo(EmcDebugInfo &debugOut);
    int isInCarvingMode();
    int setIsInCarvingMode(bool isIn);
    int setCollisionWarnTorque(int axisIndex,double  torqueIn);
    int setCollisionDetectRatio(double ratioIn);
    int setGravityVector(vector<double> gravityIn);
    int setToolMass(int toolIndex, double mass, vector<double> center);
    int setSegmentAttribute(int index,JointAttribute attributeIn);
    int setDragPara(int axisIndex,DragPara paraIn);
    int getTrigPosition(QQueue<PointPro> &trigPoseOut);
    int stopMotionTrig(int doIndex);
    int setMotionTrigPara(int doIndex,double trigPeriod);

    int getReflectLaserFilterParameter(int index,struct LaserFilterParameter &paraOut);
    int setReflectLaserFilterParameter(int index,struct LaserFilterParameter  paraIn);
    int getReflectCalculateCommonParameter(ReflectCommonDetectPara &paraOut);
    int setReflectCalculateCommonParameter( ReflectCommonDetectPara paraIn);

    int getAmclDebugInfo(AmclDebugInfo &debugInfoOut);
    int getVehicleMovingCaligrateInfo(struct VehicleMovingCaligrateInfo &infoOut);
    int setCalibrateWheelDiameter(double idealMovedLength, double actualMovedLength);
    int setCalibrateWheelDistance(double idealMovedAngle,double actualMovedAngle);
    int setAmclPara(struct AmclDebugPara paraIn);
    int getAmclPara(struct AmclDebugPara &paraOut);
    int getEncoderValue_belt(int beltIndexIn,int &encoderValueOut);
    int getEncoderValue_vision(int visionIndexIn,int &encoderValueOut);
    int forceConnectCamera(int visionIndex);
    int forceTrigCamera(int visionIndexIn, bool isHighIn);
    int getImuDeviceData(ImuDataInfo &dataOut);
    int setCoupleRelativeDistanceMonitorEnable(int followAxisId,bool isEnableIn);
    int getCoupleControlInfo(std::vector<struct CoupleConfigParameter> &infoOut);
    int getLaserScanParameter(int laserId,struct LaserScanParameterHmi &infoOut);
    int setLaserScanParameter(int laserId,struct LaserScanParameterHmi infoIn,int &returnFlag);//重启生效
    int getAgvBorderInfo(struct VehicleBorderInfo &infoOut);
    int setAgvBorderRectInfo(struct BorderRectInfo rectInfoIn,int &returnFlag);//重启生效 ros movebase使用footprint
    int getAgvSheftInfo(int sheftIndex,struct BorderRectInfo &infoOut);
    int setAgvSheftRectInfo(int sheftIndex,struct BorderRectInfo rectInfoIn,int &returnFlag);//立刻生效
    int setVehicleBorderOffset(std::vector<double> point_x, std::vector<double> point_y,int &returnFlag);//not used
    int setVehicleBorderOffset(double front_offsetIn,double back_offsetIn,double left_offsetIn,double right_offsetIn);
    int getVehicleKinematicDimension(struct VehicleKinematicDimension &infoOut);
    int setVehicleKinematicDimension(struct VehicleKinematicDimension infoIn,int &returnFlag);
    int getVehicleJogControlParameter(struct VehicleJogControlParameter &infoOut);
    int setVehicleJogControlParameter(struct VehicleJogControlParameter infoIn,int &returnFlag);
    int getVehiclePositionCalibrateInfo(struct VehiclePosition &infoOut);
    int calibrateVehicle(double xOffset,double yOffset,double rOffset);

    int getVehicleTrajectoryControlParameter(struct VehicleTrajectoryControlParameter &infoOut);
    int setVehicleTrajectoryControlParameter(struct VehicleTrajectoryControlParameter infoIn,int &returnFlag);
//    int setAxisPositionLimit(int axisId, double minPositionIn, double maxPositionIn, double approachDistanceIn);
    int setRotateEqradius(double rotate_eqradiusIn);
    int setAxisProperty(int axisId, struct AxisProperty_hmi axisPropertyIn);
    int getAxisProperty(int axisId, struct AxisProperty_hmi &axisPropertyOut);
    int getRobotCoordinateConfig(struct RobotCoordinateAxisConfig &coordinateConfigOut);
    int setRobotCoordinateConfig(struct RobotCoordinateAxisConfig coordinateConfigIn);


    int setCameraPointCoordinate(double x,double y,double z);
    int getCameraPointInBase(double &x,double &y,double &z);
    int getWeldPositionByCameraTransfer(const PointPro tcpPosition,double x,double y,double z,
                                        PointPro &pointOut);
    int calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList, std::vector<PointPro> cameraPointList,
                                  LaserCameraCalibrateResult &resultOut);
    int getLaserCameraFrameInfo(LaserCameraFrameInfo &infoOut);
    int setVelocityStyle(E_VELOCITY_TYPE velocityStypeIn);
    int setRobotArmLineMaxAcceleration(double accelerationIn);
    int setRobotArmLineMaxVelocity(double velIn);
    int setRobotArmJointMaxAcceleration(int axisIdIn,double accelerationIn);
    int setRobotArmJointMaxVelocity(int axisIdIn,double velIn);
    int activateCameraCoordinate(int index);
    int getCurrentCameraCoordinateIndex();
    int pointCoordinateToCartesian_cameraPoint(const PointPro jointPointIn,
                                               const PointPro cameraPointIn,
                                               PointPro &worldPointOut);
    int calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList, std::vector<PointPro> cameraPointList,
                                  std::vector<float> &cameraTransferOut, double &maxErrorOut);
    int getVisionTrackConfigList(std::vector<struct VisionTrackConfig> &configListOut);
    int getBeltTrackConfigList(std::vector<struct BeltTrackConfig> &configListOut);
    int changeVisionTrackConfig(int id,struct VisionTrackConfig configIn,int &returnFlag);
    int changeBeltTrackConfig(int id,struct BeltTrackConfig configIn,int &returnFlag);
    int getMapPropertyInfo(std::string mapName,struct MapPropertyInfo &infoOut);
    int setWaveAcceleration(double accelerationIn, double maxVelIn, bool isRatio);
    E_WAVE_STATUS getWaveMotionStatus();
    int startWave();
    int stopWave();
    int setWaveType(E_WAVE_TYPE typeIn, double waveWidthIn,double wavePeriodIn);
    int setWaveMotionInfo(WeaveMotionCondition infoIn);
    int setVisionTrigObjectLength(int visionIndex,double lengthIn);
    int setTrackOffset(int beltIndex, double offset_x, double offset_y);
    int getPositionRegulatorDebugInfo(int regulatorIndex,RegulatorDebugInfo& infoOut);
    int getMotorEncoderPulsePerRound(QVector<double>& pulseOut);
    int setEncoderReceiverOutput(unsigned short doIn);
    int setVisionMatchKalmanFilterPara(int filterId, double followQ, double observeR);
    int setSteeringControlKp(double kpIn, double maxSteerAdjustIn);
    int setVehicleJogDebugFlag(int flagIn);
    int setReflectMatchDebugFlag(int flagIn);
    int getAllReferenceMarks(std::vector<ReflectMarkInfo> &reflectMarksOut);
    int addReferenceMarkToMap(ReflectMarkInfo markIn);
    int saveReferenceMarkToFile(string mapFileName_yaml);
    int deleteReferenceMarkInMap(int uniqId);
    int setRosDebugFlag(int flagIn);
    int getCurrentPathInformation(VehiclePosition &startPositionOut, QVector<VehiclePosition> &targetPositionOut,
                                  int &pointIndexOut, E_NAV_COMMAND &currentPathTypeOut,
                           double &currentPathParameter1Out, double &currentPathParameter2Out);
    int resetCoupleOffset();//not used
    int getCoupleConfigInfo(int followAxisId,CoupleConfigParameter &coupleParaOut);
    int setAmclIsCalculateLaser(bool isCalculateLaserIn);
    int setCoupleControlEnable(int followAxisId,bool isEnableIn);
    int setCoupleControlOffset(int coupleId,double offfsetValueIn);
    int getVisionTrackDebugInfo(int visionIndex,VisionTrackDebugInfo &visionInfoOut);
    int getBeltTrackDebugInfo(int beltIndex,BeltTrackDebugInfo &beltTrackInfoOut);
    int modifyVisionTrackConfig(int visionIndex,VisionTrackConfig configIn);
    int modifyBeltTrackConfig(int beltIndex,BeltTrackConfig configIn);
    int runVisionTrack(int visionIndex);
    int clearVisionData(int visionIndex);
    int trigVisionOnce(int visionIndex);
    int getVisionData(int visionIndex);
    int getTrackData(int beltIndex);
    int beltTrackStart(int beltIndex);
    int beltTrackEnd(int beltIndex);

    int getEncoderReceiverData(std::vector<EncoderReceiverData> &dataListReturn);
    E_BARRIER_RUN_MODE getBarrierRunMode();
    int setBarrierRunMode(E_BARRIER_RUN_MODE modeIn);
    int setPositionPid(int axisIdIn, double pid_p, double pid_i, double pid_d, double errorLimitForIntegration, double feedforward);
    int setVelocityPid(int axisIdIn, double pid_p, double pid_i, double pid_d, double errorLimitForIntegration, double feedforward);
    int getVehiclePositionType(E_VEHICLE_POSITION_TYPE &typeOut);
    int loadMapAndSetOdometer(QString mapName, VehiclePosition position);
    int getInternalLaserDriverStatus(QVector<int> &statusOut);
    int setQrcodeDetectParameter(QrcodeDetectParameter parameterIn);
    int setQrcodeCameraId(int cameraIdIn);
    int showQrcodeDebugWindow(int id);
    int getQrcodeDebugInfo(QrcodeDebugInfo &infoOut);
    int getCu1SensorRawData(int &requestCountOut, double &l1Out, double &l2Out, double &imageOffsetOut,
                            bool &isSucessOut, int &errorCodeOut);
    int setDebugFlag(int flagIn);
    int setAgvPathFollowStrategy(E_PATH_CONTROL_STRATEGY stragetyIn);
    int getBarriorDetectMode(E_BARRIER_TETECT_MODE &modeOut);
    int getSafeCollisionInfo(int sensorType, CollisionPara &infoOut);
    int setSafeCollisionInfo(int sensorType, CollisionPara infoIn,int &returnFlag);
    bool isInCreatingMapStatus();
    int setCurrentVehicleBorderId(int idIn);
    int getCurrentVehicleBorderId();
    int setLineFittingMethod(int methodIn);
    int setIsPrintVmarkProcessData(bool isPrint);
    int isVirtualLineTrajectoryEmpty();
    int startMotorWave();
    int stopMotorWave();
    int setOriginalLaserFilterType(E_LASER_FILTER_TYPE typeIn);
    int getCurrentOriginalLaserFilterType(E_LASER_FILTER_TYPE &typeOut);
    int setVirtualLineEnableKpChange(bool isEnable);
    int moveToNavgationPoint(MovingToNavagationParameter parameterIn,int &returnFlag);
    int getVehicleSafeSpeedRatio(double &ratioOut);
    int setAgvTargetAllowOffset(float allowLineOffsetIn,float allowAngleOffsetIn);
    int changeToPositionType(E_VEHICLE_POSITION_TYPE typeIn);
    int getVMarkPositionInWorld(VehiclePosition &positionOut,bool &isSucess);
    int setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE typeIn,bool isCheck);
    int setCheckVehiclePosition_vmark(bool isCheck);
    int setCheckVehiclePosition_qrcode(bool isCheck);
    int setCheckVehiclePosition_amcl(bool isCheck);
    int setLaserIntensityFilterByType_visionMatch(int typeIn);
    int getMatchDebugInfo_vmark(VisionMatchDebugInfo &infoOut);
    int getMatchDebugInfo_reflect(ReflectDebugInfo &infoOut);
    int calculateVMarkPosition_byLaser(double baseLink_x,double baseLink_y,double baseLink_r,
                                       vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    int updateVMarkFile(QString fileName);
    int createVMark(VMarkInfo infoIn,QString vmarkFileIn);
    int getVmarkInfoList(QMap<int, VMarkInfo> &vmarkInfoOut);
    int setCurrentVMarkIndex(int indexIn);
    int setCurrentVMarkType(int currentVmarkTypeIn);
    int detectVMark_baseMap( vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    int detectVMark_laser(int vmark_index, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    int showDebugWindow_visionMatch(int id);
    int setVmarkDetectParameter(VMarkDetectParameter vmarkParameterIn);
    /**
     * @brief getTimeConsumed_pointCloudDetect
     * @param minConsumedOut (us)
     * @param currentConsumedOut (us)
     * @param maxConsumedOut (us)
     * @return
     */
    int getTimeConsumed_pointCloudDetect(double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int getTimeConsumed(int componentId,double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int resetRosMeasureTime();
    int setVirtualLinePid(double kpDistanceErrorIn, double kpAngleDistanceIn,
                          double kpOnlyRotationIn, double kpDistanceError_yIn);
    int getTargetCommandVehiclePosition(VehiclePosition &targetOut, int &pointIndexOut);
    int getCarBorder(std::vector<VehiclePosition> &carBorderOut);
    int getCarBorderOffset(std::vector<VehiclePosition> &carBorderOffsetOut);
    int getAllPointCloud_translated(std::vector<PointCloudData_3D> &pointCloudOut);
    int setUltrasonicSensorCheckEnable(bool isOn);
    int getUltrasonicSensorSafeStatus(std::vector<double> &safeStatusOut,bool &isCheckEnableOut);
    int getPointCloudSafeStatus(std::vector<std::string > &sensorNameOut,std::vector<double> &sensorSafeRatioOut,
                                bool &isCheckEnableOut);
    int setPointCloudCheckEnable(bool isOn);

    /**
     * @brief getMapNameList 获取地图文件列表
     * @param mapNameListOut　返回列表
     * @return
     */
    int getMapNameList(std::vector<string> &mapNameListOut);
    /**
     * @brief getVehicleSpeedFeedback 获取车速反馈
     * @param velocityOut　返回车速
     * @return
     */
    int getVehicleSpeedFeedback(VehicleVelocity &velocityOut);
    /**
     * @brief resetCustomOdometer_positionManage　重置用户里程表
     * @return
     */
    int resetCustomOdometer_positionManage();
    /**
     * @brief getOdometer_positionManage 获取里程信息
     * @param historyOdomOut
     * @param historyTimeOut
     * @param customOdomOut
     * @param customeTimeOut
     * @return
     */
    int getOdometer_positionManage(double &historyOdomOut, double &historyTimeOut, double &customOdomOut, double &customeTimeOut);
    int setVehicleAcceleration(double accelerationIn);
    int setVehicleAcceleration_rotate(double accelerationIn);
    int setVehiclePoseEstimation(double x,double y,double r);
    int forceAmclUpdatePartial();
    int setPclIcpWork(bool isWork);
    int setVmarkWork(bool isWork);
    int updatePclIcpDebugWindow();
    int setMatchCalculateParameter(PclCalculationParameter paraIn);
    int getMatchDebugInfo(PclMatchDebugInfo& infoOut);
    int showVisionMatchDebugWindow(bool isShowBaseMap, bool isShowBaseCompareMap,
                                   bool isShowLocalMap, bool isShowLocalCompareMap);
    int getVehiclePosition_visionMatch( PositionInfo &positionInfoOut);
    int calculateVisionMatchVehiclePosition(MapRegion baseRegionIn,MapRegion localRegionIn);
    int setBaseMapRegion_visionMatch(MapRegion baseRegionIn);
    int setLocalMapRegion_visionMatch(MapRegion localRegionIn);
    int setLocalMapLaserRegion_visionMatch(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn);
    int setBaseMapRegion_pclicp(MapRegion baseRegionIn);
    int setLocalMapRegion_pclicp(MapRegion localRegionIn);
    int setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn);

    int getVehicleFollowOffset(double &currentLineOffsetOut, double &currentAngleOffsetOut);

    int getTrackPidOutput(double &anglePidOutput, double &xPidOutput, double &yPidOutput);
    int addVirtualLineTrajectoryCommand(QVector<NavParameter> commandVectorIn);//添加一条虚拟轨迹轨迹命令
    int setMaganeticInputIndex(int indexIn);
    int writeTagData(int aliasIn, QVector<unsigned char> cardValueIn);
    int writeTagData(int aliasIn, long long cardValueIn);
    int getCurrentTagData(int aliasIn, long long &cardValueOut, bool &isCardDetected);
    int getCurrentTagData(int aliasIn, QVector<unsigned char> &cardValueOut, bool &isCardDetected);
    int getMaganeticSensorRawValue(int aliasIn,QVector<bool> &sensorRawValueOut);

    int getVehicleVelCommand(bool isJogCommand, double &xMove, double &yMove, double &rotate);
    //------------agv

    //***************AGV接口函数部分 hmi->agvBaseControl->ros********************************
    int getFrontLaserData(LaserDataInfo &frontLaserDataOut);
    int getAmclMatchScore(double &mapDistance_out, float &weight_out, std::vector<float> &pos_cov_out);
    int startNavigation();
    int stopNavigation();

    int getCurrentMapName(std::string &mapNameReturn);

    //地图
    int setMapSolver(enum E_MAP_SOLVER solverIn,
                     int laserCount,int &returnFlag);


    int createMapStart(int &returnFlag);
    int createMapEnd(std::string mapName,int &returnFlag);
    int loadMap(std::string mapName,int &returnFlag);
    int setDefaultMapName(std::string fileNameIn, int &returnFlag);

    //添加虚拟墙
    int addVitrualBlock(std::string configFilePath,int &returnFlag);
    int deleteVitrualBlock(int &returnFlag);
    int addForbidenZone(double x_start,double y_start
                        ,double x_end,double y_end,int &returnFlag);

    //定位
    int setOdomSolver(enum E_ODOM_SOLVER solverIn,int &returnFlag);
    int setLocationSolver(E_LOCATION_SOLVER solverIn, int &returnFlag);
    int getVehiclePosition(struct VehiclePosition& positionReturn);
    int getVehiclePosition_specified(PositionInfo& positionReturn,E_VEHICLE_POSITION_TYPE typeIn);
    int setVehiclePosition(struct VehiclePosition positionIn, double coveRatioIn, int &returnFlag);//给地图车初始位置

    //轨迹规划和行驶
    int setReplanCoutLimitWhenFailure(int countLimit,int &returnFlag);
    int setTrajectoryGlobalPlanSolver(enum E_TRAJECTORY_GLOBAL_SOLVER solverIn,int &returnFlag);
    int setTrajectoryLocalPlanSolver(enum E_TRAJECTORY_LOCAL_SOLVER solverIn,int &returnFlag);
    int moveToPosition(struct VehiclePosition positionIn,
                       double velocity,int &returnFlag);
    int moveFollowPath(std::vector<struct VehiclePosition> pathIn,
                       double velocity,int &returnFlag);
    int changeVelocity(double velocity,int &returnFlag);
    int getMovingStatus(enum E_GOAL_STATUS& status);
    int getRemainningTime(double &timeOut);
    int getRemainningDistance(double &distanceOut);
    int ceaseMotion(int &returnFlag);
    int startMotion(int &returnFlag);
    int cancelMotion(int &returnFlag);

    //设置
    int setPositionArrivedAccuracy(double accuracyIn,int &returnFlag);
    int setLocalPlanRange(double rangeIn,int &returnFlag);

    int setGlobalBarrierOffset(double offsetIn,int &returnFlag);
    int setLocalBarrierOffset(double offsetIn,int &returnFlag);
    int resetRosAll(int &returnFlag);

    //状态
    int getRosPackageStatus(enum E_ROS_PACKAGE package, enum E_ROS_PACKAGE_STATUS &status);

    //IMU校准
    int startImuCalibrate(int &returnFlag);
    int stopImuCalibrate(int &returnFlag);

    //信息传递
    int getMessage(std::vector<Message> &messageListReturn);

    int setWheelOdometerData(int robotIdIn,OdemeterData odemeterDataReturn,int &returnFlag);

    int getVehicleVelocityComand(int robotIdIn,VehicleVelocity &velocityComamnd);

    int setWheelMoveSpeed(int robotIdIn, std::vector<double>wheelMoveSpeed,int &returnFlag);

    int setWheelPathLength(int robotIdIn, std::vector<double>wheelPathLength,int &returnFlag);


    //-----------------------------------------------------
    int startGoZero_forUser(int axisId);
    int startGoZero_forUser();
    int haltGoZero_forUser();

    bool isAbsoulteMotorBatteryOk();
    int allowSingleAxisJog(bool enable);
    int getDebugInfo(MotionDebugInfo& infoOut);
    int getVirtuleLineDebugInfo(VirtualLineDebugInfo& infoOut);
    int controllerShutDown();
    int writePathMovingInfo();
    int setSafeSpeedRatio(double safeSpeedRatioIn, double time);
    int getWheelData(vector<double>& wheelMoveSpeed,vector<double>& wheelPathLength);

    int getOdometer(VehicleVelocity& vehicleVelFeedbackOut,
                    VehiclePosition& vehiclePositionFeedbackOut);
//    int vehicleJog(double xMove,double yMove,double rotate);
    int vehicleJog(double xMove, double yMove, double rotate);
    int vehicleJogStop();
    int vehicleAuto(double xMove, double yMove, double rotate);
    int startVehicleJogMove();
    int stopVehicleJogMove();

    int setVehicleMagneticMovePid(int idOfPid, TrackPidInfo pidInfo);

    int getJointCommand(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                                    QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut );

//    int getMaganeticDi(QVector<bool>& diOut);
    int resetVehicleMagneticMove();
    int startVehicleMagneticMove( );
    int setMageneticPassRfid(long long passRfidIn);
    int getTrackStatus(E_TRACK_STATUS &statusOut);
    int setTrackMode(E_TRACK_MODE modeIn);
    int setTrackStopCondition(E_TRACK_STOP_CONDITION conditionIn);
    int setVehicleIdealSpeed_ratio(double speedIn);
    int setVehicleIdealSpeed_mPerS(double speedIn);
    int setCheckMode(E_CHECK_MODE checkModeIn);
    int readVersion(std::vector <ComponentVersion> &componentVersionList);
    int servoOn(int axisId);//速度是０时，才能使能
    int servoOn();
    int servoOff(int axisId);
    int servoOff();
    bool isServoOn(int axisId);
    bool isServoOn();//除独立使能控制的轴外。
    int moveOneStepByVelocity(int axisId, double velocityIn);//如果非实时线程调用，会阻塞ethercat线程
    int moveOneStepByVelocity(QVector<double> velocity);
    int moveOneStepByPV(double positionIn, double velIn ,int axisId);//如果非实时线程调用，会阻塞ethercat线程
    int setMotorControlMode(int axisId, E_MOTOR_CONTROL_MODE modeIn);
    /**
     * @brief updateControllerInfoLevel2  数据更新接口,内部加锁保护数据
     * @param updateDataLv2
     * @return
     *
     */
    int updateControllerInfoLevel2( UpdateDataLv2  &updateDataLv2 );

    //// 获取类型　 0 关节    1 用户    2 工具    3 世界
    int getJogCoordinate();

    /**
     * @brief getStatus 获取当前mcControllerInterpreter的状态
     * @return
     */

    int getTrajServerStatus();

    int getStatus();
    //2017-3-10 添加
    /**
     * @brief getAutoRatio 获取当前mcControllerInterpreter的自动倍率
     * @return
     */
    double getAutoRatio();
    /**
     * @brief getRunMode
     * @return
     */
    int getRunMode();
    /**
     * @brief getMoveType
     * @return
     */
    int getMoveType();
    /**
     * @brief switchMoveType
     * @param _moveType
     */
    int switchMoveType(int moveTypeIn);
    /**
     * @brief getToolIndex
     * @return
     */
    int getToolIndex();
    /**
     * @brief getUserIndex
     * @return
     */
    int getUserIndex();

    /**
     * @brief getJointNum
     * @return
     */
    int getJointNum();
    /**
     * @brief getJointMaxPosition
     * @return
     */
    float getJointMaxPosition(int jointIndex);
    /**
     * @brief getJointMinPosition
     * @return
     */
    float getJointMinPosition(int jointIndex);
    /**
     * @brief getJointType
     * @return
     */
    int getJointType(int jointIndex);

    int getAxisInformation(std::vector<struct AxisLimit> &axisLimit,std::vector<struct AxisLimit> &extAxisLimit,
                           std::vector<int> &axisUnit,std::vector<int> &extAxisUnit,std::vector<int> &axisType,
                           std::vector<int> &extAxisType);

    /**
     * @brief addTrajectoryCommand, 获取运动指令，存放在commandQueue
     * @param tmpCommandVector
     */
    int addTrajectoryCommand(QVector <MoveParameter> tmpCommandVector);
    int transCommandQueueToCarvingModule(QVector<MoveParameter>& tmpCommandVectorIn);

    /**
     * @brief startMove, 程序运行启动
     * @return
     */
//    int setTimeAxisScale(double ratio, double timeDuration);
    int startOneModel(MC_MOTION_MODEL modelIn);
    int allStartRun();// 开始或继续运行。
    int allStartRun_fleet();//不清除命令队列
    /**
     * @brief cease, 轨迹不变暂停，启动
     * @return
     */
    int allCease();//暂停运行。
    /**
     * @brief stop
     * @return
     */
    int allStop();//停止

    /**
     * @brief quickStop
     * @return
     */
    int quickStop();
    /**
     * @brief resetAll, 清空运行数据
     * @return
     */
    int resetAll();//清空运行缓存。

    /**
     * @brief jog, 寸动
     * @param axis, 寸动轴号
     * @param direction, 寸动方向，0反向，1正向
     * @param velocityOpt, 寸动速度选项，0手动速率，1滚轮速率( 0=当前设定速度执行， 1=滚轮速度)
     * @return -1:出错，1:正确
     */
    int jog(int axis, int direction,int velocityOpt);
    /**
     * @brief setJogCoordinate, 修改寸动坐标类型，修改currentJogCoordinate
     * @param coordinateType, 坐标类型，对应MC_JOG_COORDINATE
     */
    int setJogCoordinate(int coordinateType);

    /**
     * @brief activateToolCoordinate, 设置当前活动工具坐标索引
     * @param index, 修改的工具坐标索引
     * @return 0:失败，1:成功
     */
    int activateToolCoordinate(int index);
    /**
     * @brief activateUserCoordinate, 设置当前活动用户坐标索引
     * @param index, 修改的用户坐标索引
     * @return 0:失败，1:成功
     */
    int activateUserCoordinate(int index);
    /**
     * @brief setToolCoordinate, 设置工具坐标
     * @param toolCoordinate, 工具坐标信息
     * @return
     */
    int setToolCoordinate(ToolCoordinateInfo& toolCoordinate);
    /**
     * @brief setUserCoordinate, 设置用户坐标
     * @param userCoordinate, 用户坐标信息
     * @return
     */
    int setUserCoordinate(UserCoordinateInfo& userCoordinate);
    /**
     * @brief getToolCoordinateInfo
     * @return
     */
    QVector<ToolCoordinateInfo> getToolCoordinateList();
    /**
     * @brief getUserCoordinateList
     * @return
     */
    QVector<UserCoordinateInfo> getUserCoordinateList();

    /**
     * @brief setZeroMotorPosition,根据机型，设置机器初始位置关节的电机编码值
     * @return
     */
//    int setZeroMotorPosition();//hualei todo

    /**
     * @brief getZeroMotorPosition, 将初始位置的关节的电机编码值返回，提供给ethercater标定原点
     * @return
     */
//    QVector<int> getZeroMotorPosition();//hualei todo

    /**
     * @brief palletInit        码垛信息初始化
     * @return
     */
    int palletInit();//hualei todo

    /**
     * @brief calculateInitPallet, 计算码垛初始化信息
     * @return
     */
    int calculateInitPallet();//hualei todo

    /**
     * @brief calculateNextPallet, 计算码垛下一个堆栈点／接近点／逃离点
     * @param palletNumber, 码垛号
     * @return
     */
    int calculateNextPallet(int palletNumber);//hualei todo

//    /**
//     * @brief getPalletVector,获取指定码垛号的码垛计算信息
//     * @param palletIndex，码垛号
//     * @return
//     */
//    PalletSave getPalletVector(int palletIndex);

    // 2017-4-21 添加
    /**
     * @brief setPalletPlFlag,修改指定PL寄存器时，将对应的码垛标识置１
     * @param palletIndex，码垛号
     * @return
     */
    int setPalletPlFlag(int palletIndex);//hualei todo

    /**
     * @brief setPalletFirstFlag,从停止状态到运行状态，将码垛的firstNum标志置０
     * @return
     */
    int setPalletFirstFlag(int palletIndex);//hualei todo

//    int calculatePalletRobotPos(MoveParameter& movePoint, RobotPos& f);

    /**
     * @brief setPl, 设置PalletRegister,并计算堆栈点／接近点／逃离点 ，传入参数需修改
     * @param index
     * @param row
     * @param column
     * @param layer
     * @return
     */
    int setPl(int index, int row, int column,  int layer);//直接修改RegData裏的寄存器和執行一次計算當前堆栈点／接近点／逃离点//hualei todo

    /**
     * @brief setRollerSpeed
     * @param _rollerRatio
     */
    int setRollerSpeed(float _rollerRatio);
    /**
     * @brief setManulSpeed 设置手动速率
     * @param _manualRatio
     */
    int setManulSpeed(float _manualRatio);
    /**
     * @brief setAutoSpeed 设置自动速率
     * @param _autoRatio
     */
    int setAutoSpeed(float ratioIn);

    /**
     * @brief pointCoordinateToJoint, 把pointPro转为关节类型
     * @param pointIn, 输入pointPro
     * @return 输出pointPro
     */
    PointPro pointCoordinateToJoint(PointPro pointIn);
    /**
     * @brief pointCoordinateToCartesian, 把pointPro转为笛卡尔类型
     * @param pointIn, 输入pointPro
     * @return 输出pointPro
     */
    PointPro pointCoordinateToCartesian( PointPro pointIn);
    /**
     * @brief moveToPoint 移动到点
     * @param target
     * @param pointMoveType
     */
    int moveToPoint(const MoveParameter moveParameter);//TODO  支持码垛点 //hualei todo

    int returnZeroPoint(int axisNumber, int &returnFlag);
    int moveToZeroPoint(int axisNumber, int &returnFlag);
    int moveToZeroPointBySensor(int axisNumber, int &returnFlag);

    /**
     * @brief getSoftLimit                  获取软限位数据
     * @param axisLimit                     软限位数据
     * @param axisUnit                      软限位单位
     */
    int getSoftLimit(std::vector<std::vector <float> >  &axisLimit, std::vector<int> &axisUnit);

    /**
     * @brief manualTypeSwitch
     * @param _manulType
     */
    int controlTypeSwitch(int controlTypeIn);
    int clearMsg(const Message message);//0 清除成功  -1 清除失败
    int clearAllMsg( );
    int getCurrentJointPoint_Feedback(PointPro &jointPoint);
    int getCurrentCoordinatePoint_Feedback(PointPro &coordinatePoint);
    int getJoint_Feedback(QVector<double> &jointsPosition, QVector<double> &jointsVel,
                          QVector<double> &jointAcc, QVector<double> &jointTorque);//影响实时

    float getRunningNumber();
    double getBarrierProtectRatioOfMoving();
    float getRunningNumber_virtualLineControl();
    int setRunningNumber_virtualLineControl(float numberIn);
    int setRunningNumber_robotArmControl(float numberIn);
    int setPathMovingInfo_virtualLineControl(PathMovingInfo pathInfoIn);
    int getPathMovingInfo_virtualLineControl(PathMovingInfo &infoOut);


    //弧转换为编码器值 (Vrep专用,移动关节的单位米) hualei
    int getVrepRadianToEncoder( int axis,  float &radianVec );
    //编码器值转换为弧度,(Vrep专用,移动关节的单位米)huelei
    float getVrepEncoderToRadian(int axis,  const int &encoderVec );//从ＶＲＥＰ获取ＶＥＬ数据。

   // QVector<float> getVrepEncoderToRadian(int axis, const QVector<int> &encoderVec);

    //编码器值转换为角度(移动关节的单位mm) hualei
    QVector<float> getEncoderToAngle(const QVector<int> &encoderVec );

    QVector<MotorData> getMotorFeedbackDataVector();

    int writeDataToFile();
    int isTrajectoryEmpty();

    PingPongBuffer* getPingPongBuffer();

    /**
     * @brief addMcInterpreterRegData               添加寄存器对象的指针
     * @param regData
     */
//    int addMcInterpreterRegData(RegData *regData);//hualei todo

    int getInitialStatus();

    int showMotionForm();

    int setMotionServerModel(MC_MOTION_MODEL modelIn);

    int algorithmFunctioncall(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                              QVector<MotorData> &motorCommandOut, int &returnMutexFlag);//算法循环调用函数入口。

    int validateZeroData();
    int axisLosePower();
    int axisLosePower(int axisId);
    int calculateZeroOffset(std::vector<PointPro> pointList,std::vector<double> &zeroOffsetOut
                            ,double &maxErrorOut);
    int calibrateRobotByOffset(std::vector<double> offsetIn);
    int calibrateRobot();
    int calibrateRobot(int axisId);
    bool isAxisZeroOk();
    bool isAxisZeroOk(int axisId);
    int getAxisZeroState(int axisId);
    QVector<int> getAbsoluteZeroMotorValue();
    QVector<double> getJointMechanicalZeroValue();
    int getCalibrateInformation(struct S_AXISGROUP_ZERO_INFORMATION& zeroInformationOut);



private:
    int positionRegulatorLoop(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                              QVector<MotorData> &motorCommandOut);
    int positionAndVelocityRegulatorLoop(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                                         QVector<MotorData> &motorCommandOut);
    int initial_ros();
    /**
     * @brief mcInit, 初始化
     */
    int mcInit();

    int trajectoryProcess(); //not used

    int updateCurrentPoint_FeedBack();//从关节反馈值，更新到但前坐标点。


    //---------------------------------
    int dragFromGravity();
    int gravityTest();
    int averageTorquesOffset();
    int initialServoParameter();
    int dragLoop(int axisId);
    int updateStatus();

    int checkRegisterStatus();


    int addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotIdIn,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0);

//---------------------------------------------------------------------------------------------------------



public:
    volatile float runningLineNumber; //hualei todo //0.2=算法已取点, 0.5=正在运行，0.9=(已经到目标位置),  ,下一行+0.2(避免精度丢失,不用0.1)

    /**
     * @brief status, thread运行状态  true=runnimg
     *
     */
    volatile bool isrunning;//废弃不用

//    /**
//     * @brief isTrajFinished 轨迹结束标志位
//     */
//    volatile bool isTrajFinished;

      /**
       * @brief rollerRatio 手动滚轮速率，对T1/T2都生效，改为double类型，范围0~1
       */
      volatile double rollerRatio;
      /**
       * @brief manualRatio 手动显示倍率，对T1/T2都生效，改为double类型，范围0~1
       */
      volatile double manualRatio;
      /**
       * @brief autoRatio, 自动运行倍率，double类型，范围0~1
       */
      volatile double autoRatio;
      // 2017-4-6 添加
      /**
       * @brief isPalletInit 码垛初始化成功标识位
       */
      volatile bool isPalletInit;


    /**
     * @brief jointNum 机器人关节总数量
     */
    int jointNum;

    /****************************
     * 时间：2016-12-19
     * 修改人：华磊
     * 修改内容：添加点位信息的指针,添加寄存器数据的指针
     * ***********************/
    PointData* pointData;// 自己初始化

    /***********************************
     * 时间：2017－3－30
     * 修改人：ｓｔｅｖｅｎ
     * 修改内容：添加寄存器对象的指针
     * *********************************/
    //寄存器对象指针
    RegData* mcInterpreterRegData;//外部传入


    /**
   * @brief toolCoordinateList, 工具坐标列表，存放工具坐标信息
   */
    QVector<ToolCoordinateInfo> toolCoordinateList;//hualei todo
    /**
    * @brief userCoordinateList 用户坐标列表，存放用户坐标信息
    */
    QVector<UserCoordinateInfo> userCoordinateList;//hualei todo

    /**
     * @brief currentCoordinate, 当前末端世界坐标
     */
    PointPro currentCoordinate;//华磊增加。 机器人当前坐标
    /**
     * @brief currentJoint, 当前机器人关节坐标
     */
    PointPro currentJoint;




private:
    friend class McControllerThread;
    friend class RobotBase;
    friend class EcatMasterBase;


    MotionForm *motionForm;

    /**
     * @brief robotId 机器人的ID
     */
    int robotId;


    /**
     * @brief status, 机器人运行状态，对应枚举 MC_CONTROLLER_STATUS，0 停止    1 运行    2暂停     3 出错  4初始化错误
     */
    volatile int status;
    /**
     * @brief runMode, 运行模式　( 0自动、手动调试运行)or(移动到点)or(2T1T2手动)
     */
    volatile int runMode;
    /**
     * @brief moveType, 手动调试类型，对应枚举MOVE_TYPE 0单步运行，1手动连续
     */
    volatile int moveType;
    /**
     * @brief controlType
     */
    volatile int controlType;//20170111改为控制模式(速度限制)0 T1 　 1 T2 　 2 自动 　 3 外部 4Fleet

    /**
     * @brief isRollerFlag 是否使用滚轮 0:使用手动速率 1:使用滚轮速率
     */
    volatile bool isRollerFlag;



    /**
     * @brief robotType, 机器人类型　０SCARA　１垂直串联6轴　２四轴码垛
     */
    volatile  int robotType;



    /**
     * @brief t1RatioLimit T1限速值
     */
    double t1RatioLimit;



    /**
     * @brief userCoordinateFile
     */
    QString userCoordinateFile;
    /**
     * @brief toolCoordinateFile
     */
    QString toolCoordinateFile;
//    /**
//     * @brief toolCoordinateIndex 当前工具坐标系索引
//     */
//    volatile  int toolCoordinateIndex;
//    /**
//     * @brief userCoordinateIndex,
//     */
//    volatile  int userCoordinateIndex;

    JointDecouple * jointDecouple;


private: //private variable
    TrajectoryServer* trajServer;
    RobotAttribute* robotAttribute;

    JogControl* jogControl;
    RobotCommonParameter robotParameter;

    double deltaTime;//单步长,unit is s

    double realRatio;//实际倍率
//    McTrajectory* mcTrajectory; //轨迹
//    TrajectoryScale* trajectoryScale;//轨迹时间缩放，用于暂停不变
//    JointDecouple* jointDecouple;//关节解耦
//    InternalSafe* internalSafe;//内部安全
//    MotorInterface* motorInterface;//电机控制命令
//    PalletCalculate* palletCalculate;//码垛计算功能
//    QVector<PositionLoop*> positionLoop;//位置环


    QQueue<int> linesNumberCache;//行号队列。
    QQueue<double> linesDurationCache;//行运行时间队列
    QQueue<int> linesNumber;
    QQueue<double> linesDuration;
//    DataAnalysizerDouble *analaysizerGravity;
//    DataAnalysizerDouble *analysizerTorques;
    QVector< QQueue<double> >  averageTorqueOffsetQueue;
    int isServoParameterInitialed;
    float calculatedTorques[7];
    bool isInitialOk;
    ComponentVersion versionInfo;

    MotionMessage * motionMessage;

    UserInfo userInfo;

    RosApi *g_rosApi;
    PositionRegulatorGroup *positionRegulators;
    PositionRegulatorGroup *velocityRegulators;
    int isUsePositionRegulatorStatus;
    int debugFlag;
    RobotIo *ioController;
    MathAuthorityAllowance *mathAuthority;


};

#endif // MCCONTROLLERINTERPRETER_H
